/*
putter-1.1.c
offer putter 1.1 version putter function realize
designed by lunanting

----------------------
2020-08-10 14:18:15
create file
----------------------
*/
#include "putter1-1.h"

#include "../dm556_driver.h"
#include "string.h"
#include "aos/kernel.h"


static can_node_id_t can={1,1}; //can open node
static PUTTER1_class putter1={0};
static int speed_pluse,dir=0;
static aos_sem_t sem_move;
static aos_mutex_t mux_move;


/*
putter init
param:
    slave_id:slave id
    speed:motor speed r/min
return:
    0=success
*/
int xp_putter1_init(u8 slave_id,u16 speed){
    int sta=0;
    s_profile_position_config config;

    if(xp_canopen_dev_manager_init()==-1){
        return -1;
    }
    can.node=slave_id;
    putter1.speed=speed;
    aos_msleep(2000);
    sta=enable_dm556(&can);
    if(sta!=0){
        println("DM556 driver init fail~");
        return -2;
    }
    config.motor_resolution=2000;
    config.profile_velocity=speed/60*2000;
    speed_pluse=config.profile_velocity;
    config.end_velocity=2000;
    config.profile_acceleration=20000;
    config.profile_deceleration=20000;
    sta=profile_position_config(&can,&config);
    if(sta!=0){
        println("DM556 driver config fail~");
        return -3;
    }
    putter1.location=0;
    void xp_putter1_test(void *arg);
    aos_task_new("putter test",xp_putter1_test,NULL,1024);
    void xp_putter1_move_thread(void *arg);
    aos_task_new("putter move",xp_putter1_move_thread,NULL,2100);
    if(aos_mutex_new(&mux_move)!=0){
        perr("new mux_move fail~");
        return -4;
    }
    putter1.init=1;
    return 0;
}


/*
putter move
param:
    dis:move distance mm
return:
    0=success
*/
int xp_putter1_move(int dis){
    int sta=0;
    int pluse=0;
    int H=0;
    int delay;
    u64 time_start;
	
    if(aos_mutex_lock(&mux_move,20000)!=0){
        perr("move wait timeout~");
        return 1;
    }
    if(dis==putter1.location){
        dir=0;
        aos_mutex_unlock(&mux_move);
        return 0;
    }
    else if(dis>putter1.location){
        dir=1;
        if(xp_io_read_input_pin(1,PUTTER1_limit_fw)==PUTTER1_io_touch){
            putter1.location=390;
            aos_mutex_unlock(&mux_move);
            println("putter forward limit sensor io touched !!");
            return 0;
        }
    }
    else if(dis<putter1.location){
        dir=-1;
        if(xp_io_read_input_pin(1,PUTTER1_limit_bk)==PUTTER1_io_touch){
            putter1.location=-5;
            aos_mutex_unlock(&mux_move);
            println("putter back limit sensor io touched !!");
            return 0;
        }
    }
    H=dis-putter1.location;
    pluse=H*2000*PUTTER1_1_ratio/PUTTER1_1_dis;
    sta=relative_position_move(&can,pluse);
    // if(sta!=0){
    //     aos_mutex_unlock(&mux_move);
    //     return -1;
    // }
    time_start=aos_now_ms();
    putter1.location=dis;
    //aos_msleep(abs(pluse)/(putter1.speed/60*2000)+1500);
    delay=((float)abs(pluse)/speed_pluse+((float)speed_pluse/18000))*1000;
    while(1){
        if((aos_now_ms()-time_start)>delay){
            break;
        }
        if(dir==-1){
            if(xp_io_read_input_pin(1,PUTTER1_limit_bk)==PUTTER1_io_touch){
                //putter1.location=390;
                putter1.location=-5;
                xp_putter1_stop();
                println("putter back limit sensor io touched !!");
                break;
            }
        }
        else if(dir==1){
            if(xp_io_read_input_pin(1,PUTTER1_limit_fw)==PUTTER1_io_touch){
                //putter1.location=-5;
                putter1.location=390;
                xp_putter1_stop();
                println("putter forward limit sensor io touched !!");
                break;
            }
        }

        aos_msleep(50);
    }
    dir=0;
    aos_mutex_unlock(&mux_move);
    return 0;
}

static u8 test_run=0,count=0;
/*
cycle test
*/
void xp_putter1_test(void *arg){
    while(1){
        if(test_run==0){
            aos_msleep(100);
            continue;
        }
        xp_putter1_move(250);
        aos_msleep(500);
        xp_putter1_move(0);
        aos_msleep(500);
    }
}



/*
putter1 stop
return:
    0=success
*/
int xp_putter1_stop(void){
    dir=0;
    println("putter stop~");
    return slow_down(&can,50000);
}

/*
putter move thread

*/
void xp_putter1_move_thread(void *arg){

    aos_sem_new(&sem_move,0);
    putter1.get_foot=0;
    putter1.set_foot=0;

    while(1){
        aos_sem_wait(&sem_move,AOS_WAIT_FOREVER);
        xp_putter1_move(putter1.list[putter1.get_foot++]);
        if(putter1.get_foot==5){
            putter1.get_foot=0;
        }
    }
}

/*
putter get position
*/
int xp_putter1_get_pos(void){
    return putter1.location;
}

/*
putter set move task
param:
    position:set move position
*/
void xp_putter1_set_move(int position){
    if(abs(position)>400){
        perr("set move position=%d",position);
        return;
    }
    putter1.list[putter1.set_foot++]=position;
    if(putter1.set_foot==5){
        putter1.set_foot=0;
    }
    aos_sem_signal(&sem_move);
}



/*
check zero
return:
    0=success
*/
int xp_putter1_checkZero(void){
    int sta=0;

    if(putter1.init!=1){
        perr("putter uninit~");
        return -1;
    }
    println("putter check zero ......");
    putter1.location=0;
    if(xp_putter1_move(20)!=0){sta= -2;goto END;}
    putter1.location=20;
    if(xp_putter1_move(-400)!=0){sta= -3;goto END;}
    putter1.location=0;
    if(xp_putter1_move(10)!=0){sta= -4;goto END;}
    putter1.location=0;
END:
    if(sta!=0){
        perr("putter check zero fail, sta=%d",sta);
    }
    else{
        println("putter check zero complete~");
    }
    return sta;
}



/*********************************************debug function********************************/

/*
putter1.1 debug
*/
int xp_putter1_debug(char *type,char *fun,char *param){
    int i=0;
    if(strcmp(type,"putter1")!=0)return 0;
    else if(strcmp(fun,"move")==0){
        i=atoi(param);
        if(abs(i)<400){
            xp_putter1_move(i);
        }
        else{
            println("putter set move distance over rang~");
        }
    }
    else if(strcmp(fun,"test")==0){
        if(strcmp(param,"start")==0){
            test_run=1;
        }
        else if(strcmp(param,"stop")==0){
            test_run=0;
        }
    }
    else if(strcmp(fun,"check_zero")==0){
        xp_putter1_checkZero();
    }


    else{
        return 0;
    }
    return 1;
}


//end of the file
